/*
 * Copyright (C) 2020 1km Group Holding Limited
 */

#include "putter.h"

#define PUTTER_FRONT_LIMIT 11
#define PUTTER_MIDDLE_LIMIT 10
#define PUTTER_BEHIND_LIMIT 9


can_node_id_t putter_node_id = {PORT_CAN_RESERVED, 1};

/**
 * @brief 推杆状态监测，监测外部限位输入，当限位时进行急停处理
 * 
 */ 
void xp_putter_state_check()
{
	int32_t ret = 0;
	
	while(1)
	{
		//TODO:check putter limit state,ERR process;
		/*if(){
			ret = slow_down(&putter_node_id, 500);
			if(0 != ret){
				return -1;
			}
		}*/
		
		aos_msleep(50);
	}
}

/**
 * @brief 控制推杆伸缩距离
 * 
 * @param target_position 推杆移动相对位置量,移动单位为mm
 * @return int32_t 0成功，非0表示失败
 */
int32_t xp_putter_control(int32_t target_position)
{
	int32_t rc = 0;

	//移动单位为mm，正数为前进，负数为后退
	target_position = target_position*(2000/55);

	rc = absolute_position_move (&putter_node_id, target_position);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}

/**
 * @brief 推杆初始化
 * 
 * @return int32_t 0成功，非0表示失败
 */
int32_t xp_putter_init()
{
	int32_t rc = 0;
	s_profile_position_config position_config;

	if(-1 == xp_canopen_dev_manager_init()){
		return -1;
	}

	//等待伺服进入canopen操作状态
	aos_msleep(5000);
	
	rc = enable_dm556(&putter_node_id);
	if(0 != rc){
		return -1;
	}

	position_config.motor_resolution = 2000;	
	position_config.profile_velocity = 3000;
	position_config.end_velocity = 500;
	position_config.profile_acceleration = 1000;
	position_config.profile_deceleration = 1000;
	 
	rc = profile_position_config(&putter_node_id, &position_config);
	if(0 != rc){
		return -1;
	}

	return 0;
}

/**********************DEBUG***************************/
int xp_putter_debug(char *type,char *fun,char *param)
{
	int ret = 0;
	uint32_t target_position = 0;
	
	static 	s_profile_position_config position_config = {2000, 3000, 500, 1000, 1000};

	if(strcmp(type,"putter")!=0)return 0;
	if(strcmp(fun,"motor_resolution")==0){
			position_config.motor_resolution = strtoul(param, NULL, 10);
			return 1;
	}
	else if(strcmp(fun,"profile_acceleration")==0){
			position_config.profile_acceleration = strtoul(param, NULL, 10);
			return 1;
	}
	else if(strcmp(fun,"profile_deceleration")==0){
			position_config.profile_deceleration = strtoul(param, NULL, 10);
			return 1;
	}
	else if(strcmp(fun,"profile_velocity")==0){
				position_config.profile_velocity = strtoul(param, NULL, 10);
				return 1;
	}
	else if(strcmp(fun,"end_velocity")==0){
				position_config.end_velocity = strtoul(param, NULL, 10);
				return 1;
	}
	else if(strcmp(fun,"target_position")==0){
				//移动单位为mm，正数为前进，负数为后退
				target_position = strtoul(param, NULL, 10);
		    profile_position_config(&putter_node_id, &position_config);

				xp_putter_control(target_position);
				return 1;
	}
	else if(strcmp(fun,"move")==0){
		xp_putter_control(atoi(param));
	}

	else{
			return 0;
	}
	return 1;
}


